Advertisement
Guest User

probe comp

a guest
Mar 2nd, 2023
20
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 2.15 KB | None | 0 0
  1. component probe_sim "used for testing probe routines";
  2.  
  3. pin in float x-in;
  4. pin in float y-in;
  5. pin in float z-in;
  6. pin in float probe-dia;
  7. pin out bit probe;
  8.  
  9. param rw float probe-off-x=0.005;
  10. param rw float probe-off-y=-0.007;
  11. param rw float pin-x=5;
  12. param rw float pin-y=5;
  13. param rw float pin-d=2;
  14. param rw float pin-z=-1;
  15. param rw float rect-x=-5;
  16. param rw float rect-y=-5;
  17. param rw float rect-w=3;
  18. param rw float rect-h=2;
  19. param rw float rect-z=1;
  20. param rw float rect-offset=0.25;
  21. param rw float rect-slope=0;
  22. param rw float toolset-x=-5;
  23. param rw float toolset-y=5;
  24. param rw float toolset-d=0.5;
  25. param rw float toolset-z=-5;
  26. param rw float ring-x=5;
  27. param rw float ring-y=-5;
  28. param rw float ring-z=0;
  29. param rw float ring-d=3;
  30. param rw float ring-i=2;
  31.  
  32. author "alex lait";
  33. license "GPL";
  34. function _;
  35.  
  36. ;;
  37. #include "rtapi_math.h"
  38.  
  39. FUNCTION(_){
  40.  
  41. int probetrip = 0;
  42. int xtrip = 0;
  43. int ytrip = 0;
  44.  
  45.  
  46. //pin simulation
  47. float dist_pin = pow(pow(x_in - pin_x + probe_off_x, 2) + pow(y_in - pin_y + probe_off_y, 2), 0.5);
  48. if (( dist_pin <= (probe_dia / 2 + pin_d / 2)) && (z_in <= pin_z))
  49. {
  50. probetrip = 1;
  51. }
  52.  
  53. //rectangle simulation
  54. float distx = fabs((x_in - rect_x + probe_off_x) + (y_in - rect_y + probe_off_y) * rect_slope);
  55. float disty = fabs((y_in - rect_y + probe_off_y) - (x_in - rect_x + probe_off_x) * rect_slope);
  56.  
  57.  
  58. if ((distx <= (probe_dia / 2 + rect_w / 2)) && (disty <= (probe_dia / 2 + rect_h / 2)) && (z_in <= rect_z))
  59. {
  60. if ((distx >= (rect_w / 2 - rect_offset - probe_dia / 2)) || (disty >= (rect_h / 2 - rect_offset - probe_dia / 2)))
  61. {
  62. probetrip = 1;
  63. }
  64. }
  65.  
  66. //tool setter simulation
  67. dist_pin = pow(pow(x_in - toolset_x, 2) + pow(y_in - toolset_y, 2), 0.5);
  68. if ((dist_pin <= (probe_dia / 2 + toolset_d / 2)) && (z_in <= toolset_z))
  69. {
  70. probetrip = 1;
  71. }
  72.  
  73. //ring simulation
  74. dist_pin = pow(pow(x_in - ring_x + probe_off_x, 2) + pow(y_in - ring_y + probe_off_y, 2), 0.5);
  75. if ((dist_pin <= (probe_dia / 2 + ring_d / 2)) && (dist_pin >= (ring_i / 2 - probe_dia / 2)) && (z_in <= ring_z))
  76. {
  77. probetrip = 1;
  78. }
  79.  
  80. probe = probetrip;
  81.  
  82. }
  83.  
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement